# -*- coding: utf-8 -*-
import numpy as np
import os
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def draw(points,color='red'):
    fig = plt.figure()
    ax= Axes3D(fig)
    x= points[:,0]
    y = points[:,1]
    z = points[:,2]
    ax.scatter(x,y,z)
    ax.set_zlabel('Z',fontdict={'size':15,'color':color})
    ax.set_ylabel('Y',fontdict={'size':15,'color':color})
    ax.set_xlabel('X',fontdict={'size':15,'color':color})
    plt.show()
    plt.savefig('test.png')
    return 0


def look_grid(gm,x,y):
    if gm['point_num'][x][y]!=0:
        gp = gm['points']
        gp = gp[x,y]
        p = np.array(gp)
        draw(p)
    return gm['point_num'][x][y]


def sortrows(inarray,index,desend=0):
    if desend:
        a = np.argsort(-inarray[:,index],axis=0)
    else:
        a = np.argsort(inarray[:,index],axis=0)
    outarray = inarray[a,:]
    return outarray


def read_data(filename,normalization = 0,debug_mode=0):
    print('Reading data from file...')
    a = np.loadtxt(filename)
    points = np.zeros(shape=[a.shape[0],5])
    out_points = points
    points[:,0:3] = a[:,0:3]
    minx = np.min(points[:,0])
    maxx = np.max(points[:,0])
    miny = np.min(points[:,1])
    maxy = np.max(points[:,1])
    minz = np.min(points[:,2])
    maxz = np.max(points[:,2])
    points[:,3] = range(points.shape[0])
    print('Reading data Done!')
    if normalization ==0:
        out_points = points
    else:
        print('Normalizing ...')
        out_points[:,0] = (points[:,0]-minx)/(maxx-minx)
        out_points[:,1] = (points[:,1]-miny)/(maxy-miny)
        out_points[:,2] = (points[:,2]-minz)/(maxz-minz)
        out_points[:,3:4] = points[:,3:4]
        
    #a = tuple(out_points[:,:])
    #print(a)
    #out_points = np.array(a,dtype=dtype)
    return out_points


def get_grid_map(points,grid_num):
    minx = np.min(points[:,0])
    maxx = np.max(points[:,0])+0.001
    miny = np.min(points[:,1])
    maxy = np.max(points[:,1])+0.001
    step_x = (maxx-minx)/grid_num
    step_y = (maxy-miny)/grid_num
    gm_points = []
    print('Pre-setting Done.')
    print('Total Point Number:'+str(points.shape[0]))
    px = sortrows(points,0)
    point_num = np.zeros([grid_num,grid_num],dtype=int)
    dp = np.zeros([grid_num,grid_num])
    dh = np.zeros([grid_num,grid_num])
    gh = np.zeros([grid_num,grid_num])
    ind = np.zeros([grid_num,grid_num],dtype=int)
    c = int(0)
    for i in range(grid_num):
        left = minx+i*step_x
        right = left+step_x
        msk = (px[:,0]>=left) & (px[:,0]<right)
        msklr = np.where(msk)[0]
        if any(msklr):
            mskl = np.where(msk)[0][0]
            mskr = np.where(msk)[0][-1]
            py = sortrows(px[mskl:mskr+1,:],1)
            for j in range(grid_num):
                up = miny+j*step_y
                down = up+step_y
                msky = (py[:,1]>=up) & (py[:,1]<down)
                mskud = np.where(msky)[0]
                if any(mskud):
                    msku = mskud[0]
                    mskd = mskud[-1]
                    tp = py[msku:mskd+1,:]
                    gm_points.append(tp.tolist())
                    point_num[i,j] = tp.shape[0]
                    if point_num[i,j]>1:
                        pz = sortrows(tp,2)
                        ddp = pz[1:,2]-pz[:-1,2]
                        dp[i,j] = np.max(ddp)
                        ind[i,j] = int(np.where(np.max(ddp)==ddp)[0][0])
                        dh[i,j] = pz[-1,2]-pz[0,2]
                        if dp[i,j]>0.05:
                            if (ind[i,j]<pz.shape[0]-1):
                                gh[i,j] = pz[ind[i,j],2]
                        else:
                            ll = int(pz.shape[0]/3)+1
                            gh[i,j] = np.mean(pz[0:ll,2])
                    else:
                        dh[i,j] = 0
                        dp[i,j] = 0
                        gh[i,j] = 0
                    c = c+point_num[i,j]
                    print('Processed: '+str(c)+'/'+str(points.shape[0]))
                else:
                    point_num[i,j] = 0
                    dh[i,j] = 0
                    dp[i,j] = 0
                    gh[i,j] = 0
                    gm_points.append([])
        else:
            for j in range(grid_num):
                point_num[i,j] = 0
                gm_points.append([])
                dh[i,j] = 0
                dp[i,j] = 0
                gh[i,j] = 0
    gmp = np.array(gm_points)
    gmp = gmp.reshape([grid_num,grid_num])
    
    print('Grid_map generation done.')
    
    
#    for i in range(grid_num):
#        for j in range(grid_num):
#            if point_num[i,j]>1:
#                pz = sortrows(np.array(gmp[i,j]),2)
#                ddp = pz[1:,2]-pz[:-1,2] #means diff(pz(:,2))
#                dp[i,j] = np.max(ddp)
#                ind = np.where(np.max(ddp)==ddp)[0][0]
#                dh[i,j] = pz[-1,2]-pz[0,2]
#                if dp[i,j]>0.05:
#                    if (ind<pz.shape[0]-1):
#                        gh[i,j] = pz[ind,2]
#                else:
#                    ll = int(pz.shape[0]/3)+1
#                    gh[i,j] = np.mean(pz[0:ll,2])
#            else:
#                dh[i,j] = 0
#                dp[i,j] = 0
#                gh[i,j] = 0
                
    gm ={
            'point_num': point_num,
            'points': gmp,
            'dh':dh,
            'gh':gh,
            'dp':dp,
            'dpind':ind
            }
    return gm
    
            
def find_th(gm):
    dh = gm['dh']
    ddh = dh.reshape([dh.shape[0]*dh.shape[1],])
    dp = gm['dp']
    ddp = dp.reshape([dp.shape[0]*dp.shape[1],])
    ddh = np.sort(ddh)
    ddp = np.sort(ddp)
    d1 = ddh[1:,]-ddh[:-1,]
    d1 = d1[0:9980]
    d2 = ddp[1:,]-ddp[:-1,]
    d2 = d2[0:9980]
    d1 = (d1[:,]-np.min(d1))/(np.max(d1)-np.min(d1))
    d2 = (d2[:,]-np.min(d2))/(np.max(d2)-np.min(d2))
    dd1 = d1>0.4
    dd2 = d2>0.4
    ind1 = np.where(dd1)[0][0]
    ind2 = np.where(dd2)[0][0]
    th1 = ddh[ind1]
    th2 = ddp[ind2]
    return th1,th2

def classifier(gm,points,th1,th2):
    out_points = points
    gm['class'] = np.zeros_like(gm['point_num'])
    gm['fgnum'] =  np.zeros_like(gm['point_num'])
    for i in range(gm['dh'].shape[0]):
        for j in range(gm['dh'].shape[1]):
            print('Classifying: '+str(i)+','+str(j))
            dp = gm['dp'][i,j]
            if gm['point_num'][i,j]>1:
                if (gm['dh'][i,j]>th1) and (dp>th2):
                    ind = gm['dpind'][i,j]
                    p = sortrows(np.array(gm['points'][i,j]),2)
                    p[ind+1:,4] = 1
                    b = p[ind+1:,3].astype(int)
                    gm['points'][i,j] = p.tolist()
                    gm['class'][i,j]=1
                    gm['fgnum'][i,j] = gm['point_num'][i,j]-ind
                    points[b,4] = 1
                else:
                    gm['class'][i,j] = 0
                    gm['fgnum'][i,j] = 0
            else:
                gm['class'][i,j] = 0
                gm['fgnum'][i,j] = 0
    return 0

def remove_noise(points,gm,grid_num=100):
    for i in range(1,grid_num-1):
        for j in range(1,grid_num-1):
            if (gm['class'][i,j]==1) and (gm['fgnum'][i,j]<3) and (gm['fgnum'][i,j]>0):
                re = lookaround(gm,i,j)
                if re==0:
                    p = sortrows(np.array(gm['points'][i,j]),4,1)
                    ind = np.where(p[:,4]==1)[0][-1]
                    temp = p[0:ind+1,3].astype(int)
                    points[temp,4] = 0
                    gm['class'][i,j] = 0
                    gm['fgnum'][i,j] = 0
    return 0
                
                

def lookaround(gm,x,y):
    f = (gm['fgnum']>=3)
    f = f.astype(int)
    result = sum(sum(f[x-1:x+2,y-1:y+2]))
    result = result-f[x,y]
    result = int(result>0)
    return result
                 
            
            
def sv(points):
    np.savetxt('points.txt',points)

if __name__=='__main__':
    grid_num = 100
    points = read_data('../../0.data/txtfile/data-20200619-100737.txt',1)
    #draw(points)
    gm = get_grid_map(points,grid_num)
    th1,th2 = find_th(gm)
    classifier(gm,points,th1,th2)
    remove_noise(points,gm)
    